// --------------------------------------------------------------------------------------------------------------------
// <copyright file="DepthImageVisualizationUtilities.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Vision.Visualization
{
    using System;
    using System.Drawing;
    using Microsoft.Kinect;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Vision.Cameras;
    using Microsoft.Robotics.Vision.ObjectDetection;
    using Microsoft.Robotics.Vision.Runtime.Cameras;
    
    /// <summary>
    /// Utilities for displaying depth images visually
    /// </summary>
    public static class DepthImageVisualizationUtilities
    {
        /// <summary>
        /// Create an image that displays better than the standard kinect depth values when displayed
        /// using a 16bit grayscale image.
        /// </summary>
        /// <param name="kinectAgentMessage">A kinect agent message</param>
        /// <returns>Array of 16bit values to display.</returns>
        public static ushort[] Prepare16BitDepthImageFromKinectAgentMessage(DepthCameraAgentMessage<KinectDepthCameraParameters> kinectAgentMessage)
        {
            return Prepare16BitDepthImageFromKinectAgentMessage(kinectAgentMessage);
        }

        /// <summary>
        /// Create an image that displays better than the standard kinect depth values when displayed
        /// using a 16bit grayscale image.
        /// </summary>
        /// <param name="kinectAgentMessage">A kinect agent message</param>
        /// <param name="disparityFactor">Factor used to accentuate the difference between nearby pixels</param>
        /// <returns>Array of 16bit values to display.</returns>
        public static ushort[] Prepare16BitDepthImageFromKinectAgentMessage(DepthCameraAgentMessage<KinectDepthCameraParameters> kinectAgentMessage, int disparityFactor)
        {
            const ushort NoReadingImageValue = 0;
            const ushort FurtherThanMaxDepthImageValue = 5000;
            const ushort FirstValidDepthImageValue = 10000;

            ushort[] image = new ushort[kinectAgentMessage.ImageFrame.ImageData.Length];
            double scaler = (ushort.MaxValue - FirstValidDepthImageValue) / kinectAgentMessage.Parameters.MaxDepth;
            for (int i = 0; i < kinectAgentMessage.ImageFrame.ImageData.Length; i++)
            {
                if (kinectAgentMessage.ImageFrame[i] == kinectAgentMessage.Parameters.TooNearDepth)
                {
                    image[i] = ushort.MaxValue;
                }
                else if (kinectAgentMessage.ImageFrame[i] == kinectAgentMessage.Parameters.TooFarDepth)
                {
                    image[i] = FurtherThanMaxDepthImageValue;
                }
                else if (kinectAgentMessage.ImageFrame[i] == kinectAgentMessage.Parameters.UnknownDepth)
                {
                    image[i] = NoReadingImageValue;
                }
                else
                {
                    image[i] =
                        (ushort)
                        (ushort.MaxValue - (ushort)(kinectAgentMessage.ImageFrame[i] * scaler * disparityFactor));
                }
            }

            return image;
        }

        /// <summary>
        /// Convert a RGB image for display on 16bit greyscale
        /// </summary>
        /// <param name="rgbImageArray">Input RGB array</param>
        /// <returns>Array of 16bit values to display.</returns>
        public static ushort[] Prepare16BitRGBImage(byte[] rgbImageArray)
        {
            int bytesPerPixel = Microsoft.Robotics.Vision.Cameras.ConnectKinect.RGBImageBytesPerPixel;
            if (null == rgbImageArray)
            {
                return null;
            }

            ushort[] image = new ushort[rgbImageArray.Length / bytesPerPixel];

            int pix = 0;
            int factor = ushort.MaxValue / byte.MaxValue / bytesPerPixel;

            for (int i = 0; i < rgbImageArray.Length; i += bytesPerPixel)
            {
                int pixVal = 0;
                for (int j = 0; j < bytesPerPixel; ++j)
                {
                    pixVal += rgbImageArray[i + j];
                }

                pixVal *= factor;
                image[pix++] = (ushort)pixVal;
            }

            return image;
        }

        /// <summary>
        /// Grey scale image but colors special pixels as follows:
        /// Red -  No reading
        /// Green - Further than maximum
        /// Blue - Less than minimum
        /// </summary>
        /// <param name="kinectAgentMessage">A kinect agent message</param>
        /// <param name="disparityFactor">Factor used to accentuate the difference between nearby pixels</param>
        /// <returns>Array of 16bit values to display.</returns>
        public static byte[] Prepare24BitRGBDepthImageFromKinectAgentMessage(DepthCameraAgentMessage<KinectDepthCameraParameters> kinectAgentMessage, int disparityFactor)
        {
            const ushort FirstValidDepthImageValue = 10000;

            byte[] image = new byte[kinectAgentMessage.ImageFrame.ImageData.Length * 3];
            double scaler = (ushort.MaxValue - FirstValidDepthImageValue) / kinectAgentMessage.Parameters.MaxDepth;

            int ib = 0;
            for (int i = 0; i < kinectAgentMessage.ImageFrame.ImageData.Length; i++)
            {
                if (kinectAgentMessage.ImageFrame[i] == kinectAgentMessage.Parameters.TooNearDepth)
                {
                    image[ib++] = 0;
                    image[ib++] = 0;
                    image[ib++] = 255;
                }
                else if (kinectAgentMessage.ImageFrame[i] == kinectAgentMessage.Parameters.TooFarDepth)
                {
                    image[ib++] = 0;
                    image[ib++] = 255;
                    image[ib++] = 0;
                }
                else if (kinectAgentMessage.ImageFrame[i] == kinectAgentMessage.Parameters.UnknownDepth)
                {
                    image[ib++] = 255;
                    image[ib++] = 0;
                    image[ib++] = 0;
                }
                else
                {
                    ushort v =
                        (ushort)
                        (ushort.MaxValue - (ushort)(kinectAgentMessage.ImageFrame[i] * scaler * disparityFactor));
                    byte bv = (byte)(v / byte.MaxValue);
                    image[ib++] = bv;
                    image[ib++] = bv;
                    image[ib++] = bv;
                }
            }

            return image;
        }

        /// <summary>
        /// Prepare a 24Bit RGB image for an IR image
        /// Red -  No reading
        /// Green - Further than maximum
        /// Blue - Less than minimum
        /// </summary>
        /// <param name="irFrame">IR image frame</param>
        /// <param name="disparityFactor">Factor used to accentuate the difference between nearby pixels</param>
        /// <returns>Array of 16bit values to display.</returns>
        public static byte[] Prepare24BitRGBFromIr(ImageFrame<ushort> irFrame, int disparityFactor)
        {
            return Prepare24BitRGBFromBuffer(
                                irFrame.ImageData,
                                0,
                                0,
                               ushort.MaxValue + 1,
                                disparityFactor);
        }

        /// <summary>
        /// Grey scale image but colors special pixels as follows:
        /// Red -  No reading
        /// Green - Further than maximum
        /// Blue - Less than minimum
        /// </summary>
        /// <param name="buffer">Data buffer</param>
        /// <param name="noReading">no Reading value</param>
        /// <param name="min">Minimum value</param>
        /// <param name="max">Maximum value</param>
        /// <param name="disparityFactor">Disparity factor</param>
        /// <returns>Array of 16bit values to display.</returns>
        public static byte[] Prepare24BitRGBFromBuffer(ushort[] buffer, int noReading, int min, int max, int disparityFactor)
        {
            byte[] image = new byte[buffer.Length * 3];
            double scaler = Math.Max((ushort.MaxValue - min) / max, 1);

            int ib = 0;
            for (int i = 0; i < buffer.Length; i++)
            {
                if (buffer[i] == min)
                {
                    image[ib++] = 0;
                    image[ib++] = 0;
                    image[ib++] = 255;
                }
                else if (buffer[i] == max)
                {
                    image[ib++] = 0;
                    image[ib++] = 255;
                    image[ib++] = 0;
                }
                else if (buffer[i] == noReading)
                {
                    image[ib++] = 255;
                    image[ib++] = 0;
                    image[ib++] = 0;
                }
                else
                {
                    ushort v = 
                        (ushort)
                        (ushort.MaxValue - (ushort)(buffer[i] * scaler * disparityFactor));
                    byte bv = (byte)((int)v * byte.MaxValue / ushort.MaxValue);
                    image[ib++] = bv;
                    image[ib++] = bv;
                    image[ib++] = bv;
                }
            }

            return image;
        }

        /// <summary>
        /// Add a cross at the object locations in the passed in image
        /// Note this modifies the input image
        /// </summary>
        /// <param name="input">Input image</param>
        /// <param name="poseEstimates">Estimated object poses</param>
        /// <returns>Input frame with superimposed cross</returns>
        public static ImageFrame<ushort> AddCrossToImageAtLocations(ImageFrame<ushort> input, ObjectPoseEstimate[] poseEstimates)
        {
            const int UshortBitlength = 16;
            const int CrossLength = 8;

            if (null == input)
            {
                return null;
            }

            int scale = 1;
            int crossLength = CrossLength;

            foreach (ObjectPoseEstimate e in poseEstimates)
            {
                Vector3 v = e.CameraPixelPose.Position;
                int ix = (int)Math.Round(v.X);
                int iy = (int)Math.Round(v.Y);

                if (ix > CrossLength && iy < input.Width - CrossLength && iy > CrossLength && iy < input.Height - CrossLength)
                {
                    int logScale = (int)(Math.Round((1.0 - e.Confidence) * UshortBitlength));
                    logScale = Math.Min(Math.Max(logScale, 1), 4);
                    logScale = scale++;
                    ushort color = (ushort)((input[ix, iy] + ushort.MaxValue) >> logScale);

                    for (int i = -crossLength; i < crossLength; ++i)
                    {
                        input[ix + i, iy] = color;
                        input[ix, iy + i] = color;
                    }

                    crossLength -= 2;
                }
            }

            return input;
        }
    }
}
